/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.impulserobotics;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
//We need to implement motor safety later (I think).

/**
 *
 * @author Sahil Jain
 */
public class Shooter {

    Jaguar shooterJag;
    PhysicsCalculator pc;
   
    boolean firePressed = false;
    boolean latchOpenPressed = false;
    boolean latchClosedPressed = false;

    public Shooter() {
        shooterJag = new Jaguar(Constants.SHOOTER_PWM);
        
        pc = new PhysicsCalculator();

    }

    public void alignRobot(RobotDrive robotDrive, double servoAngle, double gyroHeading) {


        double moveHeading;

        if (servoAngle > 90) {
            moveHeading = gyroHeading + (servoAngle - 90);
            while (moveHeading < gyroHeading) {
                //insert a method to align the camera
                robotDrive.drive(0, 0.5);
            }
            if (servoAngle < 90) {
                moveHeading = gyroHeading + (servoAngle - 90);
                while (moveHeading > gyroHeading) {
                    //insert a method to align the camera
                    robotDrive.drive(0, -0.5);
                }
            }
        }
    }

    public void fire(double camVAngle) {
        double speed = pc.getSpeed(camVAngle);
        double pwmOut = speed / Constants.MAX_VELOCITY;

        //turn on motor
        shooterJag.set(-pwmOut);
        Timer.delay(Constants.SHOOTER_WARMUP_DELAY);
        //open the latch
     //   latch.open();
        Timer.delay(Constants.LATCH_ROTATION_DELAY);
        //close the latch
        //latch.close();
        //turn off the motor
        shooterJag.set(0);


    }

    void update(mainRobot main, Joystick shootStick) {
        if (shootStick.getButton(Joystick.ButtonType.kTrigger) != firePressed) {
            firePressed = !firePressed;
            if (firePressed && main.aimButtonPressed) {
                rawFire();
                //fire(main.camMove.vcurAngle);
            } else {
                shooterJag.set(0.0);
            }
        }
        
//        if(main.camStick.getRawButton(Constants.LATCH_OPEN_BUTTON)!= latchOpenPressed){
//            latchOpenPressed = !latchOpenPressed;
//            if(latchOpenPressed){
//                latch.spike.set(Relay.Value.kForward);
//            }else{
//                latch.spike.set(Relay.Value.kOff);
//            }
//        }
//        if(main.camStick.getRawButton(Constants.LATCH_CLOSED_BUTTON)!= latchClosedPressed){
//            latchClosedPressed = !latchClosedPressed;
//            if(latchOpenPressed){
//                latch.spike.set(Relay.Value.kReverse);
//            }else{
//                latch.spike.set(Relay.Value.kOff);
//            }
//        }
    }

    void rawFire() {
        shooterJag.set(-1.0);
        

    }
    
     void endFire(){
        shooterJag.set(0.0);
    }
}
